#! /usr/bin/env python
'''
    

'''
from threading import Thread
from time import sleep
from motion_parser.msg import arm_cmd
import rospy

#自定义类
class ArmThread(Thread):

 def __init__(self,queue):
   Thread.__init__(self)
   #获取话题名称  ArmParserTopic: arm_parser_cmd
   topicName = rospy.get_param("ArmParserTopic","arm_parser_cmd")
   #设置发布者publisher
   self.publisher = rospy.Publisher(topicName,arm_cmd,queue_size=10)
   self.queue = queue
   self.cmd = None
   #以下是自身一些变量的定义
   #首先有两个flag用于控制yaw/pitch角度的加/减/不变
   self.yawFlag = 0
   self.pitchFlag = 0
   #随后有四个角度值，代表关节的四个角度，这四个角度应该有初始值，是由关节的当前状态所决定的,在之后每次接收命令的时候应该根据参数服务器的值更新这四个角度
   '''
    float64 baseYawAngle
    float64 basePitchAngle
    #上面两个是基座上的偏航和俯仰角度
    #这是中间关节的角度
    float64 middlePitchAngle
    #这是末端关节的角度
    float64 upperPitchAngle

    BaseYawAngle: 0
    BasePitchAngle: 45
    MiddlePitchAngle: -45
    UpperPitchAngle: 0
   
   '''
   self.baseYawAngle = float(rospy.get_param("BaseYawAngle","0"))
#    self.basePitchAngle = float(rospy.get_param("BasePitchAngle","45"))
   self.middlePitchAngle = float(rospy.get_param("MiddlePitchAngle","-45"))
#    self.upperPitchAngle = float(rospy.get_param("UpperPitchAngle","0"))
   #以下是对于两个角度的限幅
   self.baseYawAngleMax = float(rospy.get_param("BaseYawAngleMax","180"))
   self.baseYawAngleMin = float(rospy.get_param("BaseYawAngleMin","-180"))
   self.middlePitchAngleMax = float(rospy.get_param("MiddlePitchAngleMax","45"))
   self.middlePitchAngleMin = float(rospy.get_param("MiddlePitchAngleMin","-45"))
   #然后是两个不动关节的固定角度
   #BasePitchAngleManualDefault: 160
   #UpperPitchAngleManualDefault: 0
   self.basePitchAngleManualDefault = float(rospy.get_param("BasePitchAngleManualDefault","160"))
   self.upperPitchAngleManualDefault = float(rospy.get_param("UpperPitchAngleManualDefault","0"))
   #舵机运动速度，每0.1s会加上这个值，因此这个值的大小就表示了机械臂移动的速度  ServoSpeed: 0.5
   self.servoSpeed = float(rospy.get_param("ServoSpeed","0.5"))
   print("====================")
   print(self.publisher)
   print("ArmParserThread begin...")
    

 def execute(self):
      while True:
         #时刻检查更新
         self.update()
         #检查更新后根据命令实时计算位置并且调整信息
         self.calculate()
         sleep(0.1)
    #   self.epilogue()

 def run(self):
    self.execute()


 '''
    由于这个线程作为守护线程，是从程序开始到程序终止一直运行的，因此只需要时刻检查更新即可
    上一个节点（manager）传来的信息是封装好的数组，即是[a,b]的形式，其中a代表俯仰，b代表偏航
    a如果是-1代表要在每个循环将pitch角度减去一个值，而为1则表示要每次加上一个值
    b也是同理
    update函数用于更新已获得的数据，即该类应该有一些量记录当前各个关节的角度，以及是否要加/减某个角度，同时也应该提供最大角度的限制，防止其角度超出限制
    calculate函数用于根据update函数获得的信息对于已有数据进行处理，比如如果a为-1那么每个循环就减去一些值，最后还要对于范围做一个判断，将角度限制在某个范围之内
 '''
 def update(self):
     while(not self.queue.empty()):
         self.cmd = self.queue.get()
         #如果不是退出命令，那么就继续进程
        #  print(self.cmd)
         #解析命令并且刷新自身的变量
         self.fresh(self.cmd)

 def fresh(self,cmd):
     #该函数用于刷新自身的一些变量，是在接收到命令的时候调用，此时也应该根据参数服务器的值更新关节的角度位置信息
     #此时cmd传来的是一个数组[-1,1]，这个数组第一个数表示yaw，第二个数表示pitch
     self.yawFlag = cmd[0] #这里取值是-1/0/1
     self.pitchFlag = cmd[1] #这里取值是 -1/0/1
     #即每一次指令到来的时候，都更新一下当前的关节位置信息，这样开始运动后就是从之前的位置开始运动的
     #该关节位置信息应该由一个节点实时收集并管理，这样才能够保证信息的时效
     self.baseYawAngle = float(rospy.get_param("BaseYawAngle","0"))
    #  self.basePitchAngle = float(rospy.get_param("BasePitchAngle","45"))
     self.middlePitchAngle = float(rospy.get_param("MiddlePitchAngle","-45"))
    #  self.upperPitchAngle = float(rospy.get_param("UpperPitchAngle","0"))

 def calculate(self):
     if(self.yawFlag == 0 and self.pitchFlag == 0):
         #如果没有任何输入，证明不需要计算，就不会发布状态
         return
     #用于计算两个舵机角度并且发布信息
     self.baseYawAngle += self.yawFlag*self.servoSpeed  #每一个循环都计算下信息，如果不动那么yawFlag就是0，舵机角度也不会有变化
     self.middlePitchAngle += self.pitchFlag*self.servoSpeed
     #进行限制幅度
     self.limit()
     #将当前的信息装箱，准备发送
     self.publisher.publish(self.package())
     rospy.loginfo("Manual Arm Message Send!")
     

 def epilogue(self):
   print("/////////////////////////")
   rospy.loginfo("ArmParserThread ending")
   print("/////////////////////////")

 def limit(self):
     if(self.baseYawAngle < self.baseYawAngleMin):
         self.baseYawAngle = self.baseYawAngleMin
     elif(self.baseYawAngle > self.baseYawAngleMax):
         self.baseYawAngle = self.baseYawAngleMax
     if(self.middlePitchAngle < self.middlePitchAngleMin):
         self.middlePitchAngle = self.middlePitchAngleMin
     elif(self.middlePitchAngle > self.middlePitchAngleMax):
         self.middlePitchAngle = self.middlePitchAngleMax

 def package(self):
     msg = arm_cmd()
     msg.basePitchAngle = self.basePitchAngleManualDefault
     msg.baseYawAngle = self.baseYawAngle
     msg.middlePitchAngle = self.middlePitchAngle
     msg.upperPitchAngle = self.upperPitchAngleManualDefault
     return msg
     